#include "AutoControl.h"
#include "LRCSearch.h"
#include "PID.h"
#include "interface.h"
#include "main.h"
#include "motor.h"
#include "redvoid.h"
#include "stdlib.h"
#include "stm32f10x_conf.h"

// 我并不提倡外部变量写法，但是考虑到提供对外接口需要重新包装一遍，所以就先这样了

extern PID DirCtrl, SpeedCtrl;
// extern char ctrl_comm;
extern CommandPriority cmdPriority;
static char redrayStatus = VOID_NONE;
// , lastRedrayStatus = VOID_NONE;

int dirOutTbl[] = {
    7,  // L
    0,  // C
    -7, // R
    2,  // LC
    -2, // RC
    13, // LnF
    -13 // RnF
};
void autoControlInit(void) {
  PID_Init(&DirCtrl);
  PID_Config(&DirCtrl, 10, 100, 0, 10, PID_POSITIVE_EFFECT, TIM2);
  PID_LimitConfig(&DirCtrl, 200, -200);
  PID_IntLimitConfig(&DirCtrl, 20, -20);
  PID_SetTarget(&DirCtrl, 0);
  PID_SetOutput(&DirCtrl, 0);
  PID_HoldOff(&DirCtrl);

  PID_Init(&SpeedCtrl);
  PID_Config(&SpeedCtrl, 1, 0, 0, 10, PID_POSITIVE_EFFECT, TIM2);
  PID_LimitConfig(&SpeedCtrl, 100, -100);
  // PID_IntLimitConfig(&SpeedCtrl, 20, -20);
  // PID_SetTarget(&SpeedCtrl, 100);
  // PID_SetMeasured(&SpeedCtrl, 0);
  PID_SetOutput(&SpeedCtrl, 0);
  PID_HoldOn(&SpeedCtrl);

  // setDuty(0, 0);
  // Delayms(5000);
  // dutyHoldOff();

#ifdef DOWNCOUNTER_MODULE_ENABLED
#include "TIMRegister.h"
  DownCounter_Config(TIM2);
#endif
}

void autoControl(void) {
  LRCSearchState state = LRCSearch();
  // lastRedrayStatus = redrayStatus;
  redrayStatus = GetVoidStatus();

  if (cmdPriority >= cmd_RedVoid) {
    if (redrayStatus != VOID_NONE) {
      assignSpeed(0);
      assignRotateSpeed(0);
      cmdPriority = cmd_RedVoid;
    } else {
      PIDRelease();
      cmdPriority = cmd_Default;
    }
  }
  if (cmdPriority == cmd_Default) {
    // assignSpeed(100);
    // assignRotateSpeed(0);
    // int saturatedSpeed = 100 - abs((int)PID_GetOutput(&DirCtrl) / 2);
    int saturatedSpeed = 100;
    switch (state) {
      // 想要惯性滤波的差不多放这里
    case LRC_BeforeStart:
      PID_SetMeasured(&DirCtrl, 0);
      PID_SetOutput(&DirCtrl, 0);
      PID_HoldOff(&DirCtrl);
      assignSpeed(0);
      break;

    case LRC_None:
      // assignSpeed(100);
      // assignRotateSpeed(0);
      // PIDRelease();
      // PID_SetMeasured(&DirCtrl, 0);
      // assignSpeed(0);
      break;
    case LRC_L:
      PID_SetMeasured(&DirCtrl, dirOutTbl[0]);
      assignSpeed(saturatedSpeed);
      break;
    case LRC_C:
      PID_SetMeasured(&DirCtrl, dirOutTbl[1]);
      assignSpeed(saturatedSpeed);
      break;
    case LRC_R:
      PID_SetMeasured(&DirCtrl, dirOutTbl[2]);
      assignSpeed(saturatedSpeed);
      break;
    case LRC_LC:
      PID_SetMeasured(&DirCtrl, dirOutTbl[3]);
      assignSpeed(saturatedSpeed);
      break;
    case LRC_RC:
      PID_SetMeasured(&DirCtrl, dirOutTbl[4]);
      assignSpeed(saturatedSpeed);
      break;
    case LRC_All:
      PID_SetMeasured(&DirCtrl, 0);
      assignSpeed(saturatedSpeed);
      break;
    case LRC_LnF:
      PID_SetMeasured(&DirCtrl, dirOutTbl[5]);
      assignSpeed(saturatedSpeed);
      break;
    case LRC_RnF:
      PID_SetMeasured(&DirCtrl, dirOutTbl[6]);
      assignSpeed(saturatedSpeed);
      break;
    case LRC_nF:
      // PID_SetMeasured(&DirCtrl, 0);
      // assignSpeed(0);
      break;
    default:
      assert_param(0); // error
      PID_SetMeasured(&DirCtrl, 0);
      assignSpeed(0);
      break;
    }
  }
  // switch (redrayStatus) {
  // case VOID_LEFT:
  //   ctrl_comm = COMM_RIGHT;
  //   break;
  // case VOID_RIGHT:
  //   ctrl_comm = COMM_LEFT;
  //   break;
  // case VOID_BOTH:
  //   ctrl_comm = COMM_RIGHT;
  //   break;
  // case VOID_NONE:
  //   ctrl_comm = COMM_UP;
  //   break;
  // default:
  //   assert_param(0);
  //   break;
  // }
}

void manual2Auto(void) {
  PIDRelease();
  LRCReset();
  cmdPriority = cmd_Default;
}

void auto2Manual(void) {
  PIDFreeze();
  cmdPriority = cmd_Manual;
}